
	#include "Dcm.h"

/******************************************************************************
*   						MACRO DEFINITIONS
******************************************************************************/
	#define PI 							3.14159265359
	#define RADIANS_TO_DEGREES			57.29577951
	#define DEGREES_TO_RADIANS			0.017453292
	#define X_AXI						0
	#define Y_AXI						1
	#define Z_AXI						2
	#define ACEL_BUFFER_OFFSET			0
	#define GYRO_BUFFER_OFFSET			3
	
/******************************************************************************
*   					LOCAL FUNCTIONS DEFINITIONS
******************************************************************************/
	void VfnDCMNormalization(void);
	void VfnDCMDriftCorrection(void);
	void VfnEulerAngles(void);
	_iq  VfnVectorDotProduct(_iq *vector1, _iq *vector2);
	void VfnVectorCrossProduct(_iq *VectorResult, _iq *vector1,_iq *vector2);
	void VfnVectorScale(_iq *VectorResult, _iq *vector, _iq scale);
	void VfnVectorAddition(_iq *VectorResult, _iq *vector1, _iq *vector2);
	void VfnMatrixMultiplication(_iq MatrixResult[3][3], _iq Matrix1[3][3], _iq Matrix2[3][3]);
	void VfnMatrixAddition(_iq MatrixResult[3][3], _iq Matrix1[3][3], _iq Matrix2[3][3]);
	void VfnMatrixCopy(_iq MDestination[3][3], _iq MSource[3][3]);

/******************************************************************************
*   					LOCAL VARIABLE DEFINITIONS
******************************************************************************/
	_iq Gyroscope[3]  	= {_IQ(0.0), _IQ(0.0), _IQ(0.0)};
	_iq Acceleration[3] = {_IQ(0.0), _IQ(0.0), _IQ(0.0)};
	_iq AccelerationN[3] = {_IQ(0.0), _IQ(0.0), _IQ(0.0)};
	_iq Omega[3]  		= {_IQ(0.0), _IQ(0.0), _IQ(0.0)};
	_iq OmegaI[3] 		= {_IQ(0.0), _IQ(0.0), _IQ(0.0)};
	_iq OmegaP[3]		= {_IQ(0.0), _IQ(0.0), _IQ(0.0)};
	_iq YawError[3] 		= {_IQ(0.0), _IQ(0.0), _IQ(0.0)};
	volatile unsigned char dato[9];
	
/******************************************************************************
*   						EXTERNAL VARIABLES
******************************************************************************/
	volatile _iq22 Pitch,Roll,Yaw;
	DCMConfiguration gDCMConfiguration;
	_iq DCMMatrix[3][3] = 
	{
    	{ _IQ(1.0), _IQ(0.0), _IQ(0.0) },
    	{ _IQ(0.0), _IQ(1.0), _IQ(0.0) },
    	{ _IQ(0.0), _IQ(0.0), _IQ(1.0) }
	};
	

/******************************************************************************
*   					GLOBAL FUNCTIONS DEFINITIONS
******************************************************************************/

/*****************************************************************************
 * Function: 	VfnDCMInitialization
 * Description: Gains setup, value pass from a Macro definition to a struct
 * Caveats:
 * Return:     
 *****************************************************************************/		
	void VfnDCMInitialization(void)
	{
		gDCMConfiguration.KpXY = _IQ(KPXY_INIT);
		gDCMConfiguration.KiXY = _IQ(KIXY_INIT);
    	gDCMConfiguration.KpZ  = _IQ(KPZ_INIT);
    	gDCMConfiguration.KiZ  = _IQ(KIZ_INIT);
	}

/*****************************************************************************
 * Function: 	VfnDCMUpdate
 * Description: Run the complete Direction Cosine Matrix Algorithm.
 * Caveats:		Ts variable must accord to real Calling time from the application
 * Return: 		"Actualize" Roll, Pitch and Yaw variables  
 *****************************************************************************/		
	void VfnDCMUpdate(void)
	{
    	_iq DCMMatrixTemporal[3][3];	//To store intermediate data
    	_iq DCMMatrixUpdate[3][3];		//To store rotation produced in time TS
    	_iq Magnitud;
    	_iq Ts;
    	#if INPUT_TYPE == FIXED_POINT
    		_iq *AxesInputPtr;
    	#endif						

    	// Iteration Period 
    	Ts = _IQ(ITERATION_PERIOD);
    	#if INPUT_TYPE == FIXED_POINT
    		AxesInputPtr = &FilterOutputs[0];
		#endif
    	// Update Gyroscope Vector, Accelerometer Vector, Apply Signs and Offsets.
    	#if (IMU == FIRST_ORIENTATION)
    		#if INPUT_TYPE == SIGNED_SHORT
				Gyroscope[0]   = - _IQmpy(_IQdiv(_IQ16( (YAxisAngularRate - GYRO_Y_OFFSET) ),_IQ16(GYROSCOPE_SENSITIVITY	)), _IQ16(DEGREES_TO_RADIANS)); // x gyro gConfig.x_gyro_bias
    			Gyroscope[1]   = - _IQmpy(_IQdiv(_IQ16( (XAxisAngularRate - GYRO_X_OFFSET) ),_IQ16(GYROSCOPE_SENSITIVITY	)), _IQ16(DEGREES_TO_RADIANS)); // y gyro
    			Gyroscope[2]   = - _IQmpy(_IQdiv(_IQ16( (ZAxisAngularRate - GYRO_Z_OFFSET) ),_IQ16(GYROSCOPE_SENSITIVITY	)), _IQ16(DEGREES_TO_RADIANS)); // z gyro
    			Acceleration[0] =  _IQdiv(_IQ16( (YAxisAcceleration - ACEL_Y_OFFSET) )   ,   _IQ16(ACCELEROMETER_SENSITIVITY)); // x accel
    			Acceleration[1] =  _IQdiv(_IQ16( (XAxisAcceleration - ACEL_X_OFFSET) )   ,   _IQ16(ACCELEROMETER_SENSITIVITY)); // y accel
    			Acceleration[2] =  _IQdiv(_IQ16( (ZAxisAcceleration - ACEL_Z_OFFSET) )   ,   _IQ16(ACCELEROMETER_SENSITIVITY)); // z accel
    		#endif
    		#if INPUT_TYPE == FIXED_POINT
    			Gyroscope[0]   = - _IQmpy(_IQdiv(*(AxesInputPtr + Y_AXI + GYRO_BUFFER_OFFSET) - _IQ16(GYRO_Y_OFFSET),_IQ16(GYROSCOPE_SENSITIVITY	)), _IQ16(DEGREES_TO_RADIANS)); // x gyro gConfig.x_gyro_bias
    			Gyroscope[1]   = - _IQmpy(_IQdiv(*(AxesInputPtr + X_AXI + GYRO_BUFFER_OFFSET) - _IQ16(GYRO_X_OFFSET),_IQ16(GYROSCOPE_SENSITIVITY	)), _IQ16(DEGREES_TO_RADIANS)); // y gyro
    			Gyroscope[2]   = - _IQmpy(_IQdiv(*(AxesInputPtr + Z_AXI + GYRO_BUFFER_OFFSET) - _IQ16(GYRO_Z_OFFSET),_IQ16(GYROSCOPE_SENSITIVITY	)), _IQ16(DEGREES_TO_RADIANS)); // z gyro
    			Acceleration[0] =  _IQdiv(*(AxesInputPtr + Y_AXI + ACEL_BUFFER_OFFSET) - _IQ16(ACEL_Y_OFFSET)   ,   _IQ16(ACCELEROMETER_SENSITIVITY)); // x accel
    			Acceleration[1] =  _IQdiv(*(AxesInputPtr + X_AXI + ACEL_BUFFER_OFFSET) - _IQ16(ACEL_X_OFFSET)   ,   _IQ16(ACCELEROMETER_SENSITIVITY)); // y accel
    			Acceleration[2] =  _IQdiv(*(AxesInputPtr + Z_AXI + ACEL_BUFFER_OFFSET) - _IQ16(ACEL_Z_OFFSET)   ,   _IQ16(ACCELEROMETER_SENSITIVITY)); // z accel
    		#endif	
    	#endif
    	
    	#if (IMU == SECOND_ORIENTATION)
    		#if INPUT_TYPE == SIGNED_SHORT
				Gyroscope[0]    =   _IQmpy(_IQdiv(_IQ16( (YAxisAngularRate - GYRO_Y_OFFSET) ),_IQ16(GYROSCOPE_SENSITIVITY	)), _IQ16(DEGREES_TO_RADIANS)); // x gyro gConfig.x_gyro_bias
    			Gyroscope[1]    =   _IQmpy(_IQdiv(_IQ16( (XAxisAngularRate - GYRO_X_OFFSET) ),_IQ16(GYROSCOPE_SENSITIVITY	)), _IQ16(DEGREES_TO_RADIANS)); // y gyro
    			Gyroscope[2]    = - _IQmpy(_IQdiv(_IQ16( (ZAxisAngularRate - GYRO_Z_OFFSET) ),_IQ16(GYROSCOPE_SENSITIVITY	)), _IQ16(DEGREES_TO_RADIANS)); // z gyro
    			Acceleration[0] = - _IQdiv(_IQ16( (YAxisAcceleration - ACEL_Y_OFFSET) )   ,   _IQ16(ACCELEROMETER_SENSITIVITY)); // x accel
    			Acceleration[1] = - _IQdiv(_IQ16( (XAxisAcceleration - ACEL_X_OFFSET) )   ,   _IQ16(ACCELEROMETER_SENSITIVITY)); // y accel
    			Acceleration[2] =   _IQdiv(_IQ16( (ZAxisAcceleration - ACEL_Z_OFFSET) )   ,   _IQ16(ACCELEROMETER_SENSITIVITY)); // z accel
    		#endif
    		#if INPUT_TYPE == FIXED_POINT
    			Gyroscope[0]    =   _IQmpy(_IQdiv(*(AxesInputPtr + Y_AXI + GYRO_BUFFER_OFFSET) - _IQ16(GYRO_Y_OFFSET),_IQ16(GYROSCOPE_SENSITIVITY	)), _IQ16(DEGREES_TO_RADIANS)); // x gyro gConfig.x_gyro_bias
    			Gyroscope[1]    =   _IQmpy(_IQdiv(*(AxesInputPtr + X_AXI + GYRO_BUFFER_OFFSET) - _IQ16(GYRO_X_OFFSET),_IQ16(GYROSCOPE_SENSITIVITY	)), _IQ16(DEGREES_TO_RADIANS)); // y gyro
    			Gyroscope[2]    = - _IQmpy(_IQdiv(*(AxesInputPtr + Z_AXI + GYRO_BUFFER_OFFSET) - _IQ16(GYRO_Z_OFFSET),_IQ16(GYROSCOPE_SENSITIVITY	)), _IQ16(DEGREES_TO_RADIANS)); // z gyro
    			Acceleration[0] = - _IQdiv(*(AxesInputPtr + Y_AXI + ACEL_BUFFER_OFFSET) - _IQ16(ACEL_Y_OFFSET)   ,   _IQ16(ACCELEROMETER_SENSITIVITY)); // x accel
    			Acceleration[1] = - _IQdiv(*(AxesInputPtr + X_AXI + ACEL_BUFFER_OFFSET) - _IQ16(ACEL_X_OFFSET)   ,   _IQ16(ACCELEROMETER_SENSITIVITY)); // y accel
    			Acceleration[2] =   _IQdiv(*(AxesInputPtr + Z_AXI + ACEL_BUFFER_OFFSET) - _IQ16(ACEL_Z_OFFSET)   ,   _IQ16(ACCELEROMETER_SENSITIVITY)); // z accel
    		#endif	 
    	#endif
    	
		Magnitud = VfnVectorDotProduct(&Acceleration[0],&Acceleration[0]);
		//Magnitud = _IQsqrt(Magnitud);
//		AccelerationN[0] = _IQdiv(Acceleration[0],Magnitud);
//		AccelerationN[1] = _IQdiv(Acceleration[1],Magnitud);
//		AccelerationN[2] = _IQdiv(Acceleration[2],Magnitud);

		if(Magnitud > _IQ(0.81) && Magnitud < _IQ(1.21))
		{
			unsigned char c;
    		// Drift Correction
    		VfnDCMDriftCorrection();
    		c=3;
    		while(c)
    		{
    			Gyroscope[(c-1)] = Gyroscope[(c-1)] + OmegaP[(c-1)] + OmegaI[(c-1)]; 
    			c--;
    		}
		}
		
    	// Create DCM matrix, with recently data in TS time
    	DCMMatrixUpdate[0][0] =   _IQ(1.0);
    	DCMMatrixUpdate[0][1] = - _IQmpy(Gyroscope[2],Ts);   //-z
    	DCMMatrixUpdate[0][2] =   _IQmpy(Gyroscope[1],Ts);   //y
    	DCMMatrixUpdate[1][0] =   _IQmpy(Gyroscope[2],Ts);   //z
    	DCMMatrixUpdate[1][1] =   _IQ(1.0);
    	DCMMatrixUpdate[1][2] = - _IQmpy(Gyroscope[0],Ts);   //-x
    	DCMMatrixUpdate[2][0] = - _IQmpy(Gyroscope[1],Ts);   //-y
    	DCMMatrixUpdate[2][1] =   _IQmpy(Gyroscope[0],Ts);   //x
    	DCMMatrixUpdate[2][2] =   _IQ(1.0);   
  			
  		// DCMMatriz and MatrixUpdate Multiplication result on DCMMAtrixTemporal
    	VfnMatrixMultiplication(&DCMMatrixTemporal[0], &DCMMatrix[0], &DCMMatrixUpdate[0]);
    	// Update Main DCM  MAtrix
    	VfnMatrixCopy(&DCMMatrix[0],&DCMMatrixTemporal[0]);
    	// Main DCM MAtrix Normalization 
   		VfnDCMNormalization();
     	
    	VfnEulerAngles();
    	CompassHeading(Roll,Pitch);
	}

/******************************************************************************
*   					LOCAL FUNCTIONS DEFINITIONS
******************************************************************************/
/*****************************************************************************
 * Function: 	VfnDCMNormalization
 * Description: Run a few equations to normalize the DCM matrix.
 * Caveats:		
 * Return: 		N/A  
 *****************************************************************************/	
	void VfnDCMNormalization(void)
	{
    	_iq TemporalMatrix[3][3] = 
		{
    	{ _IQ(1.0), _IQ(0.0), _IQ(0.0) },
    	{ _IQ(0.0), _IQ(1.0), _IQ(0.0) },
    	{ _IQ(0.0), _IQ(0.0), _IQ(1.0) }
		};
    	_iq error=_IQ(0.0);
    	_iq Normalization=_IQ(0.0);

		//////////////////////////////////////////
		//				EQUATION 18				//
		//	error = -(X . Y)/2 = -(Xt Y)/2		//
		//////////////////////////////////////////
    	error = -(_IQmpy(VfnVectorDotProduct(&DCMMatrix[0][0],&DCMMatrix[1][0]) , _IQ(0.5))); 
		
		//////////////////////////////////////////
		//				EQUATION 19				//
		//		Xorthogonal = X + error Y		//
		//		Yorthogonal = Y + error X		//
		//////////////////////////////////////////
    	VfnVectorScale(&TemporalMatrix[0][0], &DCMMatrix[1][0], error); 
    	VfnVectorScale(&TemporalMatrix[1][0], &DCMMatrix[0][0], error); 
    	VfnVectorAddition(&TemporalMatrix[0][0], &TemporalMatrix[0][0], &DCMMatrix[0][0]); 
    	VfnVectorAddition(&TemporalMatrix[1][0], &TemporalMatrix[1][0], &DCMMatrix[1][0]); 
  		
  		//////////////////////////////////////////////////
		//					EQUATION 20					//
		//	Zorthogonal = Xorthogonal x Yorthogonal		//
		//////////////////////////////////////////////////
    	VfnVectorCrossProduct(&TemporalMatrix[2][0],&TemporalMatrix[0][0],&TemporalMatrix[1][0]);
  
  		//////////////////////////////////////////////////////////////////
		//							EQUATION 21							//
		//	Xnormalized = 0.5(3 - Xorthogonal . Xorthogonal)Xorthogonal	//
		//	Ynormalized = 0.5(3 - Yorthogonal . Yorthogonal)Yorthogonal	//
		//	Znormalized = 0.5(3 - Zorthogonal . Zorthogonal)Zorthogonal	//
		//////////////////////////////////////////////////////////////////
    	Normalization = _IQmpy( _IQ(0.5) ,(_IQ(3.0) - VfnVectorDotProduct(&TemporalMatrix[0][0],&TemporalMatrix[0][0])) ); 
    	VfnVectorScale(&DCMMatrix[0][0], &TemporalMatrix[0][0], Normalization);
    	Normalization = _IQmpy( _IQ(0.5) ,(_IQ(3.0) - VfnVectorDotProduct(&TemporalMatrix[1][0],&TemporalMatrix[1][0])) );
    	VfnVectorScale(&DCMMatrix[1][0], &TemporalMatrix[1][0], Normalization);
    	Normalization = _IQmpy( _IQ(0.5) ,(_IQ(3.0) - VfnVectorDotProduct(&TemporalMatrix[2][0],&TemporalMatrix[2][0])) );
    	VfnVectorScale(&DCMMatrix[2][0], &TemporalMatrix[2][0], Normalization);
	}

/*****************************************************************************
 * Function: 	VfnDCMDriftCorrection
 * Description: Run a few equations to eliminate Drift error from the sensors.
 * Caveats:		
 * Return: 		N/A  
 *****************************************************************************/		
	void VfnDCMDriftCorrection(void)
	{
		 _iq TemporalOmega[3];
		 _iq Error_XY[3]		= {_IQ(0.0), _IQ(0.0), _IQ(0.0)};
		 _iq HeadingXAxis,HeadingYAxis;
		 _iq CourseError;
    	//////////////////////////////////////////
    	//	PITCH AND ROLL DRIFT CORRECTION		//
    	//////////////////////////////////////////
    	VfnVectorCrossProduct(&Error_XY[0], &Acceleration[0],&DCMMatrix[2][0]);
    	VfnVectorScale(&OmegaP[0], &Error_XY[0], gDCMConfiguration.KpXY);
    	VfnVectorScale(&TemporalOmega[0], &Error_XY[0], gDCMConfiguration.KiXY);
    	VfnVectorAddition(&OmegaI[0], &OmegaI[0], &TemporalOmega[0]);
    	//////////////////////////////
    	//	YAW DRIFT CORRECTION	//
    	//////////////////////////////
    	HeadingXAxis = _IQcos(MagneticHeading);
    	HeadingYAxis = _IQsin(MagneticHeading);
    	// Yaw error calculation
    	CourseError  = _IQmpy(DCMMatrix[0][0],HeadingYAxis) - _IQmpy(DCMMatrix[1][0],HeadingXAxis); 
    	VfnVectorScale(&YawError[0],&DCMMatrix[2][0],CourseError);
    	
    	VfnVectorScale(&TemporalOmega[0],&YawError[0],gDCMConfiguration.KpZ);
    	VfnVectorAddition(&OmegaP[0],&OmegaP[0],&TemporalOmega[0]);
    	
    	VfnVectorScale(&TemporalOmega[0],&YawError[0],gDCMConfiguration.KiZ);
    	VfnVectorAddition(&OmegaI[0],&OmegaI[0],&TemporalOmega[0]);
	}
	

/*****************************************************************************
 * Function: 	VfnEulerAngles
 * Description: Obtain the PITCH, ROLL and YAw angles from the DCM matrix.
 * Caveats:		
 * Return: 		N/A  
 *****************************************************************************/	
 	void VfnEulerAngles(void)
	{
		Pitch = _IQasin(-DCMMatrix[2][0]);
    	Roll  = _IQatan2(DCMMatrix[2][1],DCMMatrix[2][2]);
    	Yaw   = _IQatan2(DCMMatrix[1][0],DCMMatrix[0][0]);
	}
		
/*****************************************************************************
 * Function: 	VfnVectorDotProduct
 * Description: Vector dot Product operation.
 * Caveats:		
 * Parameters:	Vector1, Vector2.
 * Return: 		Result.
 *****************************************************************************/	
	_iq  VfnVectorDotProduct(_iq *vector1, _iq *vector2)
	{
   	 	unsigned char comp;
   	 	_iq DotProduct = _IQ(0.0);
   	 	
   	 	for(comp = 0; comp < 3; comp++)
    	{
        	DotProduct += _IQmpy(vector1[comp], vector2[comp]);
    	} 
    	return DotProduct;    
	}

/*****************************************************************************
 * Function: 	VfnVectorCrossProduct
 * Description: Vector Cross Product operation.
 * Caveats:		
 * Parameters:	Vector Result, Vector1, Vector2.
 * Return: 		"Actualize" Vector Result.
 *****************************************************************************/
	void VfnVectorCrossProduct(_iq *VectorResult, _iq *vector1,_iq *vector2)
	{
    	VectorResult[0] = (_IQmpy(vector1[1],vector2[2]) - _IQmpy(vector1[2],vector2[1]));
    	VectorResult[1] = (_IQmpy(vector1[2],vector2[0]) - _IQmpy(vector1[0],vector2[2]));
    	VectorResult[2] = (_IQmpy(vector1[0],vector2[1]) - _IQmpy(vector1[1],vector2[0]));
	}

/*****************************************************************************
 * Function: 	VfnVectorScale
 * Description: Vector Scale operation.
 * Caveats:		
 * Parameters:	Vector Result, Vector1, Scale.
 * Return: 		"Actualize" Vector Result.
 *****************************************************************************/ 	
	void VfnVectorScale(_iq *VectorResult, _iq *vector, _iq scale)
	{
    	unsigned char comp;
    	for(comp = 0; comp < 3; comp++)
    	{
        	VectorResult[comp] = _IQmpy(vector[comp],scale); 
    	}
	}

/*****************************************************************************
 * Function: 	VfnVectorAddition
 * Description: Vector Addition operation.
 * Caveats:		
 * Parameters:	Vector Result, Vector1, Vector2.
 * Return: 		"Actualize" Vector Result.
 *****************************************************************************/ 
	void VfnVectorAddition(_iq *VectorResult, _iq *vector1, _iq *vector2)
	{
    	unsigned char comp;
    	for(comp = 0; comp < 3; comp++)
    	{
       	 	VectorResult[comp] = vector1[comp] + vector2[comp];
    	}
	}

/*****************************************************************************
 * Function: 	VfnMatrixMultiplication
 * Description: MAtrix Multiplication operation.
 * Caveats:		
 * Parameters:	Matrix Result, Matrix1, Matrix2.
 * Return: 		"Actualize" Matrix Result.
 *****************************************************************************/
	void VfnMatrixMultiplication(_iq MatrixResult[3][3], _iq Matrix1[3][3], _iq Matrix2[3][3])
	{
    	unsigned char row,column,x;
		_iq Addition[3]={_IQ(0.0), _IQ(0.0), _IQ(0.0)};
		
    	for(row = 0; row < 3; row++)
    	{
        	for (column = 0; column < 3; column++)
        	{
            	for(x = 0; x < 3; x++)
            	{
                	Addition[x] = _IQmpy(Matrix1[row][x],Matrix2[x][column]);
            	} 
            	MatrixResult[row][column] = Addition[0] + Addition[1] + Addition[2];
        	}
    	}
	}

/*****************************************************************************
 * Function: 	VfnMatrixAddition
 * Description: MAtrix Addition operation.
 * Caveats:		
 * Parameters:	Matrix Result, Matrix1, Matrix2.
 * Return: 		"Actualize" Matrix Result.
 *****************************************************************************/
	void VfnMatrixAddition(_iq MatrixResult[3][3], _iq Matrix1[3][3], _iq Matrix2[3][3])
	{
    	unsigned char row,column;
    	
    	for (row = 0; row < 3; row++) 
    	{
        	for (column = 0; column < 3; column++)
        	{
          	  MatrixResult[row][column] = Matrix1[row][column] + Matrix2[row][column];
        	} 
    	}
	}

/*****************************************************************************
 * Function: 	VfnMatrixCopy
 * Description: MAtrix copy.
 * Caveats:		
 * Parameters:	Matrix Destination, Matrix Source.
 * Return: 		"Actualize" Matrix Destination.
 *****************************************************************************/	
	void VfnMatrixCopy(_iq MDestination[3][3], _iq MSource[3][3])
	{
		unsigned char row,column;
		for (row = 0; row < 3; row++) 
    	{
        	for (column = 0; column < 3; column++)
        	{
          	  MDestination[row][column] = MSource[row][column];
        	} 
    	}
	}
